#ifndef RVIZ_PLANNER__COSTMAP_HPP_
#define RVIZ_PLANNER__COSTMAP_HPP_

#include <fstream>
#include <vector>
#include <queue>
#include <unordered_map>
#include <cmath>

namespace rviz_planner
{
    class Costmap
    {
    public:
        Costmap();
        ~Costmap();

        void initTestData();

        bool worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my);
        void mapToWorld(unsigned int mx, unsigned int my, double &wx, double &wy);

        unsigned int getSizeInCellsX();
        unsigned int getSizeInCellsY();

    private:
        unsigned int size_x_;
        unsigned int size_y_;
        double resolution_;
        double origin_x_;
        double origin_y_;
    };
} // namespace rviz_planner
#endif // RVIZ_PLANNER__COSTMAP_HPP_